#include "dataProcess.h"
#include "main.h"
#include "usart.h"
#include "control.h"
#include "WT61.h"
uint8_t uartBuffer[10];
uint8_t rx_buf;
uint8_t rx_sta;
uint8_t rx_flag = 0; //接收标志位
uint16_t data[4]={0,0,0,0}; //dir[0] x dir[1] y
uint8_t gyro_buf;


// void HAL_UART_RxEventCallback(UART_HandleTypeDef *huart)
// {
//     if (huart->Instance == USART3) {
//         if(HAL_UARTEx_GetRxEventType(huart)!= HAL_UART_RXEVENT_TC) return ;
//         //包头包尾处理

//     }
// }
uint8_t checkData(){
    static uint8_t check = 0;
    if(check==0){
        check = 1;
        return 0; //第一次接收，等待下一个字节
    }
    static uint16_t checkData[4];
    for(int i=0;i<4;i++){
        checkData[i] = data[i];
    }
    if(data[0]-checkData[0] == 50 && data[1]-checkData[1] == 50 && data[2]-checkData[2] == 50 && data[3]-checkData[3] == 50){
        check = 0; //数据变化过大，重置检查状态
        return 1; //数据不合法
    }


}
float horiz_speed ;
float verti_speed ;
int flag = 0; //标志位，表示是否需要停止电机

uint16_t jiguang_real_x = 127;
uint16_t jiguang_real_y = 127;
uint16_t background_x = 0xffff;
uint16_t background_y = 0xffff;


uint16_t task_state = 0; //任务状态

uint16_t Shun_find = 0; //顺时针查找标志
uint16_t Ni_find = 0; //逆时针查找标志

uint16_t find_speed = 60; //查找速度

uint16_t GetJiGuangRealX(void){
    return jiguang_real_x;
}

/// @brief 微調激光坐標
/// @param x 
/// @param y 
void AddJiGuangRealXY(int16_t addx, int16_t addy, int16_t addspeed){
    jiguang_real_x += addx;
    jiguang_real_y += addy;
    find_speed += addspeed;
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
		if(huart->Instance == USART1)
		{
			//处理陀螺仪数据
            CopeSerialData(GYRO_data);
            WT61_ReadData();
		}
    if(huart->Instance == USART3)
    {
        if(rx_buf == 0xAA && rx_sta == 0){
            rx_sta = 1;
            HAL_UART_Receive_IT(&huart3, &rx_buf, 1);
            return;
        }
        else if(rx_buf == 0xAA && rx_sta == 1){
            rx_sta = 2;
            HAL_UART_Receive_IT(&huart3, uartBuffer, 10);
            return;
        }
        else if(rx_sta == 2){
            if(uartBuffer[8]==0xBB&&uartBuffer[9]==0xBB){
                //处理数据
                data[0] = ((uint16_t)uartBuffer[1] << 8) | uartBuffer[0]; //x坐标
                data[1] = ((uint16_t)uartBuffer[3] << 8) | uartBuffer[2]; //y坐标
                data[2] = ((uint16_t)uartBuffer[5] << 8) | uartBuffer[4]; //背景框x坐标
                data[3] = ((uint16_t)uartBuffer[7] << 8) | uartBuffer[6]; //背景框y坐标
                rx_flag = 1; //数据接收完成标志
                
                background_x = data[2];
                background_y = data[3];

                //控制算法
                

                if(data[0]==0xffff){
                    data[0] = jiguang_real_x;
                }
                if(data[1]==0xffff){
                    data[1] = jiguang_real_y;
                }
				if(data[2]==0xffff||data[3]==0xffff){
					data[2] = jiguang_real_x;data[3] = jiguang_real_y;
                    flag=1;
				}

                if (Shun_find)
                {
                    data[2] = jiguang_real_x+find_speed;
                    flag=0;
                }
                if (Ni_find)
                {
                    data[2] = jiguang_real_x-find_speed;
                    flag=0;
                }
                
                
                // task_state=1;

                if(flag==0&&(task_state == 1 || task_state == 2|| task_state == 3)) //如果任务状态为2或3，且flag为0
                {

                int horiz_err = data[0]-data[2];
                static float init_kp=0;
                static float init_ki=0;
//                if(abs(horiz_err)>10){
//									if(init_ki==0&&init_kp==0){
//                    init_kp = Motor_horiz.Motor_pid.Kp;
//                    init_ki = Motor_horiz.Motor_pid.Ki;
//									}
//                    Motor_horiz.Motor_pid.Kp = 30;
//                    Motor_horiz.Motor_pid.Ki = 10;
//                }
//                else{
//                    if(init_kp != 0 || init_ki != 0){
//                        Motor_horiz.Motor_pid.Kp = init_kp;
//                        Motor_horiz.Motor_pid.Ki = init_ki;
//                    }
//                }
                horiz_speed = -PIDCalculate(&Motor_horiz.Motor_pid,horiz_err,0); 
                verti_speed = -PIDCalculate(&Motor_Verti.Motor_pid,data[1]-data[3],0);

                float w = getGyroZ(); //陀螺仪角速度
                float gyro_compensation=0;
                if(abs(w)<10){
                    gyro_compensation = 0;
                }
                else{
                    gyro_compensation = -w/360*6400;
                }
                moto_set_speed(horiz_speed+gyro_compensation, Motor_horiz.Motor);
                moto_set_speed(verti_speed, Motor_Verti.Motor);
                }
                else{
                    flag = 0;
                    horiz_speed = 0;
                    verti_speed = 0;
                    moto_set_speed(horiz_speed, Motor_horiz.Motor);
                    moto_set_speed(verti_speed, Motor_Verti.Motor);
                }			
            }
            else{
                //数据错误
            }
            rx_sta = 0;
            HAL_UART_Receive_IT(&huart3, &rx_buf, 1);
        }
        else{
            //接收错误，重置状态
            rx_sta = 0;
            HAL_UART_Receive_IT(&huart3, &rx_buf, 1);
        }
    }
}